/*
 * HalcyonV1.cpp
 *
 * Created: 28.04.2012 10:11:25
 *  Author: maxim.karban@gmail.com
 */ 

#include "HalcyonV1.h"

#include <stdint.h>
#include <stdbool.h>

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include <math.h>
#include <util/delay.h>

#include "string.h"

#include "i2c_master.h"
#include "SMD500_BMP085.h"
#include "MMA7455.h"
#include "GPS_MTK.h"
#include "UART.h"
#include "frsky.h"
#include "filter.h"

uint8_t mmabuf[6] = {0, 0, 0, 0, 0, 0};


static bool pressure_data_ready = false;
static bool accel_data_ready = false;
static bool accel_ready = false;
static bool altitude_ready = false;

volatile int16_t accel_x = 0;
volatile int16_t accel_y = 0;
volatile int16_t accel_z = 0;
volatile int16_t accel_x_prev = 0;
volatile int16_t accel_y_prev = 0;
volatile int16_t accel_z_prev = 0;

volatile int16_t H_bp = 0;
volatile uint16_t H_ap = 0;

#define MMAGetAccel() i2c_read(mmabuf, 0x3A, 0x00, 6, 0, 1)

//timings variables for 40ms intervals
#define SYSTIME_1S      25
#define SYSTIME_200MS   5

int systime = SYSTIME_200MS;

bool mark1s = false;
bool mark200ms = false;

enum SensorState{
	SS_INIT_1,
	SS_INIT_2,
	SS_NORMAL_1,
	SS_NORMAL_2
};

SensorState sensorState = SS_INIT_1;

//each 40ms for SMD500
//each 10ms for BMP085
//in case BMP085 we can get even faster to get better response time
//ISR(TIMER1_OVF_vect){//, ISR_NOBLOCK){
ISR(TIMER1_COMPA_vect){//, ISR_NOBLOCK){
  //sei();
	switch(sensorState){
		case SS_INIT_1:
      BSStartPress();
      sensorState = SS_INIT_2;
      break;
		case SS_INIT_2:
      BSReadPress();
      BSStartTemp();
      sensorState = SS_NORMAL_1;
      break;
		case SS_NORMAL_1:
      //UT = 32135;//
	    BSReadTemp();
      BSStartPress();
      sensorState = SS_NORMAL_2;
	    //debug
		  //TIMSK1 &= ~(1 << OCIE1A);
		  //debug
      pressure_data_ready = true;
      //MMAGetAccel();
      //accel_data_ready = true;
      break;
		case SS_NORMAL_2:
      BSReadPress();
      BSStartTemp();
      sensorState = SS_NORMAL_1;
      pressure_data_ready = true;
      //MMAGetAccel();
      //accel_data_ready = true;
      break;
	}
  
  if(--systime <= 0){
    systime = SYSTIME_1S;
    mark1s = true;
  }
  if((systime/SYSTIME_200MS)*SYSTIME_200MS == systime){
    mark200ms = true;
  }
  //_delay_ms(40);
  //cli();  
}


int main( void )
{
  DDRC |= 1 << 1;//gps reset
  PORTC |= 1 << 1;
  
  
/*	gps_gsa_is_ready = true;
	gps_gsv_is_ready = true;
	gps_gga_is_ready = false;
	gps_rmc_is_ready = false;
	
*/
  // (TX) as output
  DDRD |= 1 << 1; //PD1 = TX
  //setup uddr and others
  UCSR0A = 0x00;
  UCSR0B = 
//    (1 << RXCIE0) | 
//    (1 << RXEN0) | 
    (1 << TXEN0) | 
    (0 << UCSZ02);
  
//	gpsMTKReset();
	
	ACTIVATE_FRSKY_TX;

	BSInit();

	BSReadCal();

  MMAInit();
  
  TCCR1A = (0 << COM1A1) | (0 << COM1A0) | (0 << WGM11) | (0 << WGM10) ;// 0b1000000;//0x80
  TCCR1B = (0 << WGM13) | (1 << WGM12) | (1 << CS12) | (0 << CS11) | (1 << CS10);// 0b00000101;//0x05
  TCCR1C = 0x00;

  OCR1A = 312;
  
  TIMSK1 = 1 << OCIE1A;
  
  sei();
  
  while(1){
    if(pressure_data_ready){
		  BSCalcAltitude();
      
      //H = filterA1_lpf(H);
      H = filterA1_avg(H);
	    H_bp = H/100;
      H_ap = abs(H)-abs(H_bp)*100;
		  pressure_data_ready = false;
      altitude_ready = true;
	  }

    /*if(gps_new_data_available){
      gps_decoder_buffer_is_ready_to_write = false;
			ParseGPS();
			gps_decoder_buffer_is_ready_to_write = true;
			gps_decoder_buffer_is_ready_to_read = false;
			gps_new_data_available = false;
		}*/		
    
    if(accel_data_ready){
		
 				accel_x = ((mmabuf[1] << 8) | (mmabuf[0])) << 6;
        accel_x >>= 2;
				accel_y = ((mmabuf[3] << 8) | (mmabuf[2])) << 6;
        accel_y >>= 2;
				accel_z = ((mmabuf[5] << 8) | (mmabuf[4])) << 6;
        accel_z >>= 2;
        
#define kf 0.02
        accel_x = (short)(accel_x_prev*(1-kf)+accel_x*kf);
        accel_x_prev = accel_x;
        accel_y = (short)(accel_y_prev*(1-kf)+accel_y*kf);
        accel_y_prev = accel_y;
        accel_z = (short)(accel_z_prev*(1-kf)+accel_z*kf);
        accel_z_prev = accel_z;
        accel_data_ready = false;
        accel_ready = true;
    }
    
    
    if(mark200ms){
      
      
      
      if(accel_ready){
        //frskycnt = 0;  
        //printFrSkyValue(accel_x, 0x24);
        //printFrSkyValue(accel_y, 0x25);
        //printFrSkyValue(accel_z, 0x26);
        //accel_ready = 0;
      }

      if(altitude_ready){
        printFrSkyValue(H_bp, 0x10);
        printFrSkyValue(H_ap, 0x21);
        altitude_ready = 0;
      }

      frskycnt = 0;  
      frskybuf[frskycnt++] = 0x5E; //end of frame
      PrintBuf(frskybuf, frskycnt);
      mark200ms = false;
	  

    }

//    if(gps_sentences_is_ready)
	{
//      gps_gsa_is_ready = true;
	  	//gps_gsv_is_ready = true;
  		//gps_gga_is_ready = false;
		  //gps_rmc_is_ready = false;
				
					
		  if(gps_status == 'A')
      {
        /*printFrSkyValue(gps_speed_bp, FRSKY_GPS_SPEED_BP);
        printFrSkyValue(gps_speed_ap, FRSKY_GPS_SPEED_AP);
        printFrSkyValue(gps_alt_bp, FRSKY_GPS_ALTITUDE_BP);
        printFrSkyValue(gps_alt_ap, FRSKY_GPS_ALTITUDE_AP);
        printFrSkyValue(gps_lon_bp, FRSKY_GPS_LONG_BP);
        printFrSkyValue(gps_lon_ap, FRSKY_GPS_LONG_AP);
        printFrSkyValue(gps_lon_ew, FRSKY_GPS_LONG_EW);
        printFrSkyValue(gps_lat_bp, FRSKY_GPS_LAT_BP);
        printFrSkyValue(gps_lat_ap, FRSKY_GPS_LAT_AP);
        printFrSkyValue(gps_lat_ns, FRSKY_GPS_LAT_NS);
        
        frskycnt = 0;  
        frskybuf[frskycnt++] = 0x5E; //end of frame
        PrintBuf(frskybuf, frskycnt);
		*/
      }
    }

  }
  
	
	
}		  						
				
		  	
	
